#include <utils/topic.h>

#include "data_proxy.h"
#include "transforms3d/transforms3d.h"

#include <functional>

using namespace interface::msg;

namespace ui {

static std::vector<std::string> getCameras(void)
{
    static std::vector<std::string> cameras {
        "camera0",
        "camera1",
        "camera2",
        "camera3",
        "camera4",
        "camera5",
        "camera6",
    };

    return cameras;
}

DataProxy::DataProxy(std::string name)
    : Node(std::move(name))
{
    using namespace std::placeholders;

    uiMasterObjectPainter_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(uiMasterObjectTopic, 10);
    uiMultiFusedObjectPainter_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(uiMultiFusedObjectTopic, 10);
    uiCameraFusedObjectPainter_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(uiCameraFusedObjectsTopic, 10);
    uiLanePainter_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(uiLanePointsTopic, 10);

    for (auto& i : getCameras()) {
        uiImagePainters_.emplace(i, this->create_publisher<sensor_msgs::msg::Image>(uiImageBaseTopic + i, 10));
        uiCameraFusedImagePainters_.emplace(i, this->create_publisher<sensor_msgs::msg::Image>(uiCameraFusedImageBaseTopic + i, 10));
    }

    imagesSubscriber_ = this->create_subscription<Images>(imagesTopic, 10, std::bind(&DataProxy::onImages, this, _1));
    cameraFusedImagesSubscriber_ = this->create_subscription<Images>(cameraFusedImagesTopic, 10, std::bind(&DataProxy::onCameraFusedImages, this, _1));
    cameraFusedSubscriber_ = this->create_subscription<Objects>(cameraFusedObjectsTopic, 10, std::bind(&DataProxy::onCameraFusedObjects, this, _1));
    multiFusedSubscriber_ = this->create_subscription<Objects>(multiFusedObjectsTopic, 10, std::bind(&DataProxy::onMultiFusedObjects, this, _1));
    multiLanePointsSubscriber_ = this->create_subscription<MultiLanePoints>(lanePointsTopic, 10, std::bind(&DataProxy::onLanePoints, this, _1));
    eventSubscriber_ = this->create_subscription<Event>(eventTopic, 10, std::bind(&DataProxy::onEvent, this, _1));

    timer_ = this->create_wall_timer(std::chrono::milliseconds(200), [=] {
        drawMainCar();
        updateAlarmVehicleMap();
    });
}

void DataProxy::decorateAlarmVehicle(ObjectModelInfo& objectModelInfo, AlarmVehicleLevel alarmVehicleLevel)
{
    switch (alarmVehicleLevel) {
    case AlarmVehicleLevel::SEVERITY:
        objectModelInfo.r = 1.0;
        objectModelInfo.g = 0;
        objectModelInfo.b = 0;
        break;
    case AlarmVehicleLevel::WARNING:
        objectModelInfo.r = 1.0;
        objectModelInfo.g = 1.0;
        objectModelInfo.b = 0;
        break;
    case AlarmVehicleLevel::NOTIFY:
        break;
    default:
        break;
    }
}

void DataProxy::updateAlarmVehicleMap(void)
{
    std::lock_guard<std::mutex> locker(alarmVehicleMapLock_);

    auto msTs = std::chrono::system_clock::now().time_since_epoch().count() / 1000000;

    for (auto begin = alarmVehicleMap_.begin(); begin != alarmVehicleMap_.end();) {
        if (abs(begin->second.second - msTs) >= 1000) {
            begin = alarmVehicleMap_.erase(begin);
        } else {
            begin++;
        }
    }
}

DataProxy::ObjectModelInfo DataProxy::getObjectModelInfo(int32_t type)
{
    static std::unordered_map<int32_t, ObjectModelInfo> objectModelInfos {
        { -1, ObjectModelInfo { .resource = "package://ui/mesh/master.stl", .scaleX = 0.8, .scaleY = 0.8, .scaleZ = 0.8, .r = 141 / 225.0, .g = 146 / 225.0, .b = 157 / 225.0, .lifetimeSec = 0, .lifetimeNsec = 0 } }, // 主车

        { 0, ObjectModelInfo { .resource = "package://ui/mesh/traffic_cone.obj", .r = 1.0, .g = 0.0, .b = 0.0 } }, // 未知

        { 1, ObjectModelInfo { .resource = "package://ui/mesh/car.stl", .scaleX = 0.8, .scaleY = 0.8, .scaleZ = 0.8, .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 轿车

        { 2, ObjectModelInfo { .resource = "package://ui/mesh/minibus.obj", .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 面包车

        { 3, ObjectModelInfo { .resource = "package://ui/mesh/moto.obj", .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 摩托车

        { 4, ObjectModelInfo { .resource = "package://ui/mesh/van.obj", .scaleY = 0.6, .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 厢式货车

        { 5, ObjectModelInfo { .resource = "package://ui/mesh/van.obj", .scaleY = 0.6, .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 卡车

        { 6, ObjectModelInfo { .resource = "package://ui/mesh/bus.obj", .scaleY = 0.6, .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 公交车

        { 101, ObjectModelInfo { .resource = "package://ui/mesh/persion.stl", .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 行人

        { 102, ObjectModelInfo { .resource = "package://ui/mesh/bicycle.obj", .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 自行车

        { 103, ObjectModelInfo { .resource = "package://ui/mesh/bicycle.obj", .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 三轮车

        { 104, ObjectModelInfo { .resource = "package://ui/mesh/traffic_cone.obj", .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 锥桶

        { 105, ObjectModelInfo { .resource = "package://ui/mesh/traffic_cone.obj", .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 三角牌

        { 106, ObjectModelInfo { .resource = "package://ui/mesh/dog.stl", .r = 143 / 225.0, .g = 143 / 225.0, .b = 143 / 225.0 } }, // 动物
    };

    if (objectModelInfos.find(type) == objectModelInfos.end()) {
        return objectModelInfos[0];
    } else {
        return objectModelInfos[type];
    }
}

void DataProxy::onImages(std::shared_ptr<Images> msg)
{
    for (auto& i : msg->images) {
        if (uiImagePainters_.find(i.header.frame_id) == uiImagePainters_.end()) {
            RCLCPP_ERROR(this->get_logger(), "Unsupported camera:%s", i.header.frame_id.c_str());
            continue;
        }

        uiImagePainters_[i.header.frame_id]->publish(i);
    }
}

void DataProxy::onCameraFusedImages(std::shared_ptr<Images> msg)
{
    for (auto& i : msg->images) {
        if (uiCameraFusedImagePainters_.find(i.header.frame_id) == uiCameraFusedImagePainters_.end()) {
            RCLCPP_ERROR(this->get_logger(), "Unsupported camera:%s", i.header.frame_id.c_str());
            continue;
        }

        uiCameraFusedImagePainters_[i.header.frame_id]->publish(i);
    }
}

std::string printObject(const Object& object)
{
    std::stringstream ss;

    ss << "frameId:" << object.header.frame_id << ",id:" << object.id << ",x:" << object.x << ",y:" << object.y << ",heading:" << object.heading << ",speed:" << object.speed;

    return ss.str();
}

void DataProxy::onCameraFusedObjects(std::shared_ptr<Objects> msg)
{
    static auto lastMilTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
    auto currMilTs = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
    std::cout << currMilTs - lastMilTs << std::endl;
    lastMilTs = currMilTs;

    static std::unordered_map<std::string, std::vector<double>> cameraColor {
        { "camera0", { 255 / 255.0, 0 / 255.0, 0 / 255.0, 0.5 } }, // 赤 255,0,0
        { "camera1", { 255 / 255.0, 128 / 255.0, 0 / 255.0, 0.5 } }, // 橙 255,128,0
        { "camera2", { 255 / 255.0, 255 / 255.0, 0 / 255.0, 0.5 } }, // 黄 255,255,0
        { "camera3", { 0 / 255.0, 255 / 255.0, 0 / 255.0, 0.5 } }, // 绿 0,255,0
        { "camera4", { 0 / 255.0, 255 / 255.0, 255 / 255.0, 0.5 } }, // 青 0,255,255
        { "camera5", { 0 / 255.0, 0 / 255.0, 255 / 255.0, 0.5 } }, // 蓝 0,0,255
        { "camera6", { 128 / 255.0, 0 / 255.0, 255 / 255.0, 0.5 } }, // 紫 128,0,255
    };

    auto markerArray = objectsToMarkerArray(msg);

    for (auto& i : markerArray.markers) {
        i.color.a = 0.5;

        if (cameraColor.find(i.ns) == cameraColor.end()) {
            continue;
        }

        auto color = cameraColor[i.ns];
        i.color.r = color.at(0);
        i.color.g = color.at(1);
        i.color.b = color.at(2);
    }

    uiCameraFusedObjectPainter_->publish(std::move(markerArray));
}

void DataProxy::cleanLastMultiFusedForUi(visualization_msgs::msg::MarkerArray& markerArray, std::shared_ptr<Objects> msg)
{
    for (auto& i : lastIds_) {
        visualization_msgs::msg::Marker marker;
        marker.action = visualization_msgs::msg::Marker::DELETE;
        marker.id = i.first;
        marker.ns = i.second;
        markerArray.markers.insert(markerArray.markers.begin(), marker);
    }

    lastIds_.clear();

    for (auto& i : msg->objects) {
        lastIds_.emplace(i.id, i.header.frame_id);
    }
}

void DataProxy::addCleanMarker(visualization_msgs::msg::MarkerArray& markerArray)
{
    visualization_msgs::msg::Marker marker;
    marker.id = -100;
    marker.action = visualization_msgs::msg::Marker::DELETEALL;

    markerArray.markers.insert(markerArray.markers.begin(), std::move(marker));
}

void deal(std::shared_ptr<Objects> msg)
{
BEGIN:
    for (auto& i : msg->objects) {
        bool flags { false };
        for (auto j = msg->objects.begin(); j != msg->objects.end();) {
            if (i.id == j->id) {
                j++;
                continue;
            }
            auto distance = std::hypot(i.x - j->x, i.y - j->y);
            if (distance <= 20) {
                j = msg->objects.erase(j);
                flags = true;
            } else {
                j++;
            }
        }
        if (flags == true) {
            goto BEGIN;
        }
    }
}

void DataProxy::onMultiFusedObjects(std::shared_ptr<Objects> msg)
{
    auto markerArray = objectsToMarkerArray(msg);

    uiMultiFusedObjectPainter_->publish(std::move(markerArray));
}

void DataProxy::onLanePoints(std::shared_ptr<MultiLanePoints> msg)
{
    visualization_msgs::msg::MarkerArray markerArray;

    addCleanMarker(markerArray);

    for (size_t i = 0; i < msg->multi_lane_points.size(); i++) {
        visualization_msgs::msg::Marker lineStrip;

        lineStrip.header.frame_id = "map";
        lineStrip.header.stamp = this->now();
        lineStrip.ns = "Lane";
        lineStrip.action = visualization_msgs::msg::Marker::ADD;
        lineStrip.pose.orientation.w = 1.0;
        lineStrip.lifetime.sec = 1;

        lineStrip.id = i;

        lineStrip.scale.x = 0.1;

        lineStrip.color.r = 146 / 255.0;
        lineStrip.color.g = 159 / 255.0;
        lineStrip.color.b = 178 / 255.0;
        lineStrip.color.a = 1;

        lineStrip.type = visualization_msgs::msg::Marker::LINE_STRIP;

        for (auto& j : msg->multi_lane_points.at(i).lane_points) {
            geometry_msgs::msg::Point point;
            point.x = j.x;
            point.y = j.y;
            lineStrip.points.push_back(point);
        }

        markerArray.markers.emplace_back(std::move(lineStrip));
    }

    uiLanePainter_->publish(std::move(markerArray));
}

void DataProxy::onEvent(std::shared_ptr<Event> msg)
{
    static std::unordered_map<std::string, std::function<void(std::shared_ptr<Event>)>> registerHandler {
        { "ocw", [=](std::shared_ptr<Event> msg) {
             std::lock_guard<std::mutex> locker(alarmVehicleMapLock_);
             for (auto& i : msg->vehicles) {
                 alarmVehicleMap_[i.trackid].first = (AlarmVehicleLevel)msg->level;
                 alarmVehicleMap_[i.trackid].second = std::chrono::system_clock::now().time_since_epoch().count() / 1000000;
             }
         } }
    };

    if (registerHandler.find(msg->type) == registerHandler.end()) {
        return;
    }

    registerHandler[msg->type](msg);
}

void DataProxy::drawMainCar(void)
{
    // 添加主车
    Object object;
    object.header.frame_id = "masterCar";
    object.header.stamp = this->now();
    object.id = -1;
    object.type = -1;
    object.x = 0;
    object.y = 0;
    object.heading = 0;

    auto tmp = std::make_shared<Objects>();
    tmp->objects.emplace_back(std::move(object));

    auto markerArray = objectsToMarkerArray(tmp, false);
    // 添加车道线
    visualization_msgs::msg::Marker leftLineStrip;
    leftLineStrip.header.frame_id = "map";
    leftLineStrip.header.stamp = this->now();
    leftLineStrip.ns = "mainLane";
    leftLineStrip.action = visualization_msgs::msg::Marker::ADD;
    leftLineStrip.pose.orientation.w = 1.0;

    leftLineStrip.id = -2;

    leftLineStrip.type = visualization_msgs::msg::Marker::LINE_STRIP;

    leftLineStrip.scale.x = 0.1;

    leftLineStrip.color.r = 146 / 255.0;
    leftLineStrip.color.g = 159 / 255.0;
    leftLineStrip.color.b = 178 / 255.0;
    leftLineStrip.color.a = 1;

    auto rightLineStrip = leftLineStrip;
    rightLineStrip.id = -3;

    double laneWidth = 3.75;
    geometry_msgs::msg::Point startPoint, endPoint;
    startPoint.x = laneWidth / 2 * -1;
    endPoint.x = laneWidth / 2 * -1;
    startPoint.y = -50;
    endPoint.y = 100;

    leftLineStrip.points.push_back(startPoint);
    leftLineStrip.points.push_back(endPoint);

    startPoint.x = laneWidth / 2;
    endPoint.x = laneWidth / 2;
    rightLineStrip.points.push_back(startPoint);
    rightLineStrip.points.push_back(endPoint);

    markerArray.markers.emplace_back(std::move(leftLineStrip));
    markerArray.markers.emplace_back(std::move(rightLineStrip));
    uiMasterObjectPainter_->publish(std::move(markerArray));
}

std::shared_ptr<DataProxy::AlarmVehicleLevel> DataProxy::isAlarmVehicle(int64_t id)
{
    std::lock_guard<std::mutex> locker(alarmVehicleMapLock_);

    if (alarmVehicleMap_.find(id) == alarmVehicleMap_.end()) {
        return nullptr;
    }

    return std::make_shared<AlarmVehicleLevel>(alarmVehicleMap_[id].first);
}

visualization_msgs::msg::MarkerArray DataProxy::objectsToMarkerArray(std::shared_ptr<Objects> msg, bool isAddCLeanMarker)
{
    visualization_msgs::msg::MarkerArray markerArray;

    if (isAddCLeanMarker) {
        addCleanMarker(markerArray);
    }

    for (auto& i : msg->objects) {
        visualization_msgs::msg::Marker marker;

        auto objectModelInfo = getObjectModelInfo(i.type);

        auto isAlarm = isAlarmVehicle(i.id);
        if (isAlarm != nullptr) {
            decorateAlarmVehicle(objectModelInfo, *isAlarm);
        }

        marker.id = i.id;

        marker.ns = i.header.frame_id;
        marker.header.frame_id = "map";
        marker.header.stamp = this->now();

        marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
        marker.mesh_resource = objectModelInfo.resource;

        marker.action = visualization_msgs::msg::Marker::ADD;

        builtin_interfaces::msg::Duration lifetime;
        lifetime.sec = objectModelInfo.lifetimeSec;
        lifetime.nanosec = objectModelInfo.lifetimeNsec;
        marker.lifetime = lifetime;

        marker.pose.position.x = objectModelInfo.x + i.x;
        marker.pose.position.y = objectModelInfo.y + i.y;
        marker.pose.position.z = objectModelInfo.z + i.z;

        // 此些类型航向角不变
        if (i.type == 3 || i.type == 101 || i.type == 102 || i.type == 103) {
            i.heading = 0;
        }

        auto quat = TransForms::EulerAngle2Quat(objectModelInfo.eulerX, objectModelInfo.eulerY, -1 * (objectModelInfo.eulerZ + i.heading));

        marker.pose.orientation.x = quat.x();
        marker.pose.orientation.y = quat.y();
        marker.pose.orientation.z = quat.z();
        marker.pose.orientation.w = quat.w();

        // set scale
        marker.scale.x = objectModelInfo.scaleX;
        marker.scale.y = objectModelInfo.scaleY;
        marker.scale.z = objectModelInfo.scaleZ;

        // set color.
        marker.color.r = objectModelInfo.r;
        marker.color.g = objectModelInfo.g;
        marker.color.b = objectModelInfo.b;
        marker.color.a = objectModelInfo.a;

        markerArray.markers.emplace_back(std::move(marker));
    }

    return markerArray;
}
}
